Camera Calibration

1. Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.

In [1]:
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg

%matplotlib inline


def getpoints(imgfiles, nx, ny, drawcorners=False, allowoffbyone=True):
    '''Construct image points and object points from chessboard calibration image files'''
    imgpoints = []
    objpoints = []
    imgsize = None

    # prepare constant object points to append if corners found in an image
    objp = np.zeros((nx*ny,3), dtype=np.float32)
    objp[:,:2] = np.mgrid[:nx,:ny].T.reshape(-1,2)

    # Process images and build imgpoints list
    for imgfile in imgfiles:
        
        img = mpimg.imread(imgfile)
        gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
        this_imgsize = gray.shape[::-1]
        if imgsize is None:
            imgsize = this_imgsize # store (width, height) for use by cv2.calibrateCamera()
        else:
            msg = 'Size mismatch: {}. {} != {}.'.format(imgfile, imgsize, gray.shape[::-1])
            # a couple of the images have an extra pixel in each dimension, but it should be
            # ok since the corners are within the smaller dimension range?
            # Thus, let's optionally allow it.
            allowedsizes = [this_imgsize]
            if allowoffbyone:
                allowedsizes.append((this_imgsize[0] - 1, this_imgsize[1] - 1))
            assert imgsize in allowedsizes, msg

        ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)

        # If corners found, draw corners, and append corresponding img/obj points
        if ret:
            
            # Optionally plot the images with corners drawn
            if drawcorners:
                cv2.drawChessboardCorners(img, (nx, ny), corners, ret)
                plt.figure()
                plt.imshow(img)
                plt.title(imgfile + ' corners')
            
            objpoints.append(objp)
            imgpoints.append(corners)
            
    
    return objpoints, imgpoints, imgsize
In [2]:
import glob
import os

# Make a list of calibration image files
imgfiles = glob.glob('camera_cal/calibration*.jpg')

# Get corresponding object/image points assuming 9x6 checkerboard
objpoints, imgpoints, imgsize = getpoints(imgfiles, nx=9, ny=6, drawcorners=True)

# Compute calibration matrix and distortion coefficients
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, imgsize, None, None)

# Load a calibration image and undistort it
examplefile = imgfiles[0]
img = mpimg.imread(examplefile)
dst = cv2.undistort(img, mtx, dist, None, mtx)

# Plot the before and after
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15,30))
ax1.imshow(img)
ax1.set_title(examplefile, fontsize=20)
ax2.imshow(dst)
ax2.set_title(examplefile + ' undistorted', fontsize=20)
name, ext = os.path.splitext(os.path.basename(examplefile))
fig.savefig('examples/' + name + '_undistortion' + ext, bbox_inches='tight')

assert ret, "Problem Calibrating Camera!"

Pipeline step analysis/visualization examples (test images)

2. Apply a distortion correction to raw images.

In [3]:
examplefiles = glob.glob('test_images/test*.jpg')

for examplefile in examplefiles:

    # Load a test image and undistort it
    img = mpimg.imread(examplefile)
    dst = cv2.undistort(img, mtx, dist, None, mtx)

    # Plot the before and after
    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15,30))
    ax1.imshow(img)
    ax1.set_title(examplefile, fontsize=15)
    ax2.imshow(dst)
    ax2.set_title(examplefile + ' undistorted', fontsize=15)
    name, ext = os.path.splitext(os.path.basename(examplefile))
    fig.savefig('examples/' + name + '_undistortion' + ext, bbox_inches='tight')

3. Use color transforms, gradients, etc., to create a thresholded binary image.

In [4]:
def get_color_channel(img, cvtmode, channel_idx=0):
    '''Apply desired color conversion and return the desired channel'''
    
    # Convert color space
    img_cvt = cv2.cvtColor(img, cvtmode)

    # Select desired color channel
    img_cvt = np.atleast_3d(img_cvt) # in case it's grayscale
    channel_img = img_cvt[:, :, channel_idx]
    
    return channel_img


def deriv_mag_thresh(img, orient='x', sobel_kernel=3, thresh=(0, 255),
                     cvtmode=cv2.COLOR_RGB2GRAY, channel_idx=0):
    '''Apply Sobel x or y, take the absolute value and apply thresholds
       to produce boolean pixel mask'''
    
    # Convert colorspace and select desired channel
    ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
    
    # Take the partial derivative wrt x or y given orient = 'x' or 'y'
    if orient=='x':
        sobel = cv2.Sobel(ch_img, cv2.CV_64F, 1, 0)
    elif orient=='y':
        sobel = cv2.Sobel(ch_img, cv2.CV_64F, 0, 1)
    else:
        raise ValueError("orient can be 'x' or 'y'")
    
    # Take the absolute value of that partial derivative
    abs_sobel = np.abs(sobel)
    
    # Scale to 8-bit (0 - 255) then convert to type = np.uint8
    scaled_sobel = np.uint8(255 * abs_sobel / np.max(abs_sobel))
    
    # Create a boolean mask of 'True' pixels using thresh interval
    boolean_mask = (scaled_sobel > thresh[0]) & (scaled_sobel < thresh[1])
    
    return boolean_mask


def grad_mag_thresh(img, sobel_kernel=3, mag_thresh=(0, 255),
                    cvtmode=cv2.COLOR_RGB2GRAY, channel_idx=0):
    '''Apply Sobel x and y, compute the magnitude of the gradient
       and apply thresholds to produce boolean pixel mask'''

    # Convert colorspace and select desired channel
    ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
    
    # Compute the x and y components of the gradient
    sobelx = cv2.Sobel(ch_img, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(ch_img, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    
    # Calculate the gradient magnitude 
    mag = np.sqrt(sobelx**2 + sobely**2)
    
    # Scale to 8-bit (0 - 255) and convert to type = np.uint8
    scaled_mag = np.uint8(255 * mag / mag.max())
    
    # Create a boolean mask of 'True' pixels using thresh interval
    boolean_mask = (scaled_mag >= mag_thresh[0]) & (scaled_mag <= mag_thresh[1])

    return boolean_mask


def grad_dir_thresh(img, sobel_kernel=3, thresh=(0, np.pi/2),
                    cvtmode=cv2.COLOR_RGB2GRAY, channel_idx=0):
    '''Apply Sobel x and y, compute the gradient direction and apply
       thresholds to produce boolean pixel mask'''

    # Convert colorspace and select desired channel
    ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
    
    # Take the gradient in x and y separately
    sobelx = cv2.Sobel(ch_img, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(ch_img, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    
    # Take the absolute value of the x and y gradients
    abs_sobelx = np.abs(sobelx)
    abs_sobely = np.abs(sobely)
    
    # Use np.arctan2(abs_sobely, abs_sobelx) to calculate the direction of the gradient 
    angle = np.arctan2(abs_sobely, abs_sobelx)
    
    # Create a boolean mask of 'True' pixels using thresh interval
    boolean_mask = (angle > thresh[0]) & (angle < thresh[1])

    return boolean_mask


def color_thresh(img, thresh=(0, 255), cvtmode=cv2.COLOR_RGB2HLS,
                 channel_idx=2):
    '''Optionally convert to another colorspace, then threshold the desired channel
       to produce a boolean pixel mask.
       By default it takes an RGB image, converts it to HLS and thresholds the
       S channel to produce a boolean pixel mask'''
    
    # Convert colorspace and select desired channel
    ch_img = get_color_channel(img, cvtmode, channel_idx=channel_idx)
    
    # Create a boolean mask of 'True' pixels using thresh interval
    boolean_mask = (ch_img > thresh[0]) & (ch_img <= thresh[1])

    return boolean_mask


def get_pixel_mask(img):
    '''Use colorspace conversions, color thresholds and gradients to produce images mapped
       to max/min pixel values representing a boolean mask of selected pixels. Returns 
       gradient/color combined as well as stacked for debug/analysis.'''
    
    # Create boolean masks using each technique
    x_mask = deriv_mag_thresh(img, orient='x', sobel_kernel=3, thresh=(20, 100),
                              cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
    y_mask = deriv_mag_thresh(img, orient='y', sobel_kernel=3, thresh=(20, 100),
                              cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
    mag_mask = grad_mag_thresh(img, sobel_kernel=9, mag_thresh=(30, 100),
                               cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
    dir_mask = grad_dir_thresh(img, sobel_kernel=15, thresh=(0.7, 1.3),
                               cvtmode=cv2.COLOR_RGB2HLS, channel_idx=1)
    color_mask = color_thresh(img, thresh=(170, 255), cvtmode=cv2.COLOR_RGB2HLS, channel_idx=2)

    # Combine the partial derivative and gradient magnitude and direction masks
    grad_mask = (x_mask) | (mag_mask & dir_mask)

    # Combine gradient and color masks
    grad_color_mask = grad_mask | color_mask
    
    # Create black and white image of the fully combined mask
    grad_color_img = np.uint8(grad_color_mask*255)
    
    # Create stacked image of color and grad masks for visualizing the contribution of each
    grad_color_img_stacked = np.dstack((np.zeros_like(grad_mask, np.uint8),
                                np.uint8(grad_mask*255),
                                np.uint8(color_mask*255)))
    
    return grad_color_img, grad_color_img_stacked
    
    
In [5]:
examplefiles = glob.glob('test_images/test*.jpg')

for examplefile in examplefiles:
    
    # Load a test image
    img = mpimg.imread(examplefile)
    
    # Correct for camera distortion
    dst = cv2.undistort(img, mtx, dist, None, mtx)

    # Get binary mask images
    combined, stacked = get_pixel_mask(dst)

    # Plot the before and after
    fig, ax = plt.subplots(2, 2, figsize=(16,11))
    ax[0,0].imshow(dst)
    ax[0,0].set_title(examplefile, fontsize=15)
    ax[0,1].imshow(dst)
    ax[0,1].set_title(examplefile + ' undistorted', fontsize=15)
    ax[1,0].imshow(stacked)
    ax[1,0].set_title(examplefile + ' undistorted\nstacked gradient & color masks', fontsize=15)
    ax[1,1].imshow(combined, cmap='gray')
    ax[1,1].set_title(examplefile + ' undistorted\ncombined gradient & color masks', fontsize=15)
    
    name, ext = os.path.splitext(os.path.basename(examplefile))
    fig.savefig('examples/' + name + '_binary' + ext, bbox_inches='tight')

4. Apply a perspective transform to rectify binary image ("birds-eye view").

In [6]:
import tabulate

# src points rectangle vertices
srcpoints = np.float32(
    [[(imgsize[0] / 2) - 60, imgsize[1] / 2 + 100],
    [((imgsize[0] / 6) - 10), imgsize[1]],
    [(imgsize[0] * 5 / 6) + 40, imgsize[1]],
    [(imgsize[0] / 2 + 64), imgsize[1] / 2 + 100]])

# dst points rectangle vertices
dstpoints = np.float32(
    [[(imgsize[0] / 4), 0],
    [(imgsize[0] / 4), imgsize[1]],
    [(imgsize[0] * 3 / 4), imgsize[1]],
    [(imgsize[0] * 3 / 4), 0]])

print(tabulate.tabulate({'srcpoints':srcpoints, 'dstpoints':dstpoints},
                        headers='keys', tablefmt='pipe'))

# Compute perspective transform matrix, M
M = cv2.getPerspectiveTransform(srcpoints, dstpoints)

# Compute inverse perspective transform matrix, Minv (used later to draw computed lane onto scene)
Minv = cv2.getPerspectiveTransform(dstpoints, srcpoints)
| dstpoints     | srcpoints                       |
|:--------------|:--------------------------------|
| [ 320.    0.] | [ 580.  460.]                   |
| [ 320.  720.] | [ 203.33332825  720.        ]   |
| [ 960.  720.] | [ 1106.66662598   720.        ] |
| [ 960.    0.] | [ 704.  460.]                   |
In [7]:
straight_examplefiles = glob.glob('test_images/straight_lines*.jpg')

# Create subplot axes with appropriate shape and size
fig, ax = plt.subplots(2, 2, figsize=(16, 11))

for idx, examplefile in enumerate(straight_examplefiles):
    
    # load image
    img = mpimg.imread(examplefile)
    
    # correct for camera distortion
    undist = cv2.undistort(img, mtx, dist, None, mtx)
    
    # Warp image perspective to a top-down view
    warped = cv2.warpPerspective(undist, M, imgsize, flags=cv2.INTER_LINEAR)
    
    # Draw source and destination point "rectangles"
    cv2.polylines(undist,[srcpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=4)
    cv2.polylines(warped,[dstpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=8)
    
    # Plot them
    ax[idx,0].imshow(undist)
    ax[idx,0].set_title(examplefile, fontsize=15)
    ax[idx,1].imshow(warped)
    ax[idx,1].set_title(examplefile + ' warped', fontsize=15)
    
fig.savefig('examples/straight_lines_warped.jpg', bbox_inches='tight')
In [8]:
examplefiles = glob.glob('test_images/*.jpg')

for examplefile in examplefiles:
    
    # Load a test image
    img = mpimg.imread(examplefile)
    
    # Correct for camera distortion
    dst = cv2.undistort(img, mtx, dist, None, mtx)

    # Get binary mask image
    combined,_ = get_pixel_mask(dst)
    
    # Warp binary mask images
    combined_warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
    
    # Draw source and destination point "rectangles"
    combined = np.dstack([combined]*3)
    combined_warped = np.dstack([combined_warped]*3)
    cv2.polylines(combined,[srcpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=4)
    cv2.polylines(combined_warped,[dstpoints.reshape((-1,1,2)).astype(np.int32)],True,(255,0,0), thickness=8)
    
    # Plot them
    fig, ax = plt.subplots(1, 2, figsize=(16, 5.5))
    ax[0].imshow(combined)
    ax[0].set_title(examplefile + ' mask', fontsize=15)
    ax[1].imshow(combined_warped)
    ax[1].set_title(examplefile + ' mask warped', fontsize=15)
    
    name, ext = os.path.splitext(os.path.basename(examplefile))
    fig.savefig('examples/' + name + '_binary_warped' + ext, bbox_inches='tight')

5. Detect lane pixels and fit to find the lane boundary.

In [9]:
def get_windowed_pts(image, window_center, window_width, window_height, out_img=None, level=0):
    '''Get pixel (y,x) points within the window'''
    
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = np.transpose(image.nonzero())
    nonzeroy = nonzero[:,0]
    nonzerox = nonzero[:,1]
    
    # Identify window boundaries in x and y (and right and left)
    win_y_low = np.int(image.shape[0] - (level + 1)*window_height)
    win_y_high = np.int(image.shape[0] - level*window_height)
    win_x_low = np.int(window_center - window_width//2)
    win_x_high = np.int(window_center + window_width//2)
    
    if out_img is not None:
        # Draw the windows on the visualization image
        cv2.rectangle(out_img,(win_x_low,win_y_low),(win_x_high,win_y_high),(0,255,0), 3) 
    
    # Identify the nonzero pixels in x and y within the window
    windowed_pts = nonzero[(nonzeroy >= win_y_low) & (nonzeroy < win_y_high) &
                           (nonzerox >= win_x_low) & (nonzerox < win_x_high), :]
    
    return windowed_pts


def draw_polyfit(lane_pts, out_img):
    '''Draw 2nd degree polynomial fit of lane_pts on out_img if out_img is not None'''

    if out_img is None:
        return # no image to draw on, do nothing
    
    # Fit a second order polynomial to each
    fit = np.polyfit(lane_pts[:, 0], lane_pts[:, 1], 2)

    # Generate x and y values for plotting the polynomials
    ploty = np.arange(0, out_img.shape[0],10)
    fitx = np.polyval(fit, ploty)

    # Draw the polynomials
    pts = np.stack((np.int32(np.round(fitx)), np.int32(ploty)), axis=1).reshape((-1,1,2))
    cv2.polylines(out_img,[pts], False,(255,255,0), 10)


def compute_conv_signal(image, window_width, window_height, level):
    
    # convolve the window into the vertical slice of the image
    image_layer = np.sum(image[int(image.shape[0]-(level+1)*window_height):
                               int(image.shape[0]-level*window_height),:], axis=0)
    conv_signal = np.convolve(np.ones(window_width), image_layer)
    
    return conv_signal


def compute_window_center(image, prev_center, window_width, margin, conv_signal):

    # Find the best centroid by using past center as a reference
    # Use window_width/2 as offset because convolution signal reference is at right side of
    # window, not center of window
    offset = window_width/2
    min_index = int(max(prev_center+offset-margin,0))
    max_index = int(min(prev_center+offset+margin,warped.shape[1]))
    center = np.argmax(conv_signal[min_index:max_index])+min_index-offset
    
    return center


def get_lane_pts_conv(image, window_width, window_height, margin, out_img=None):
    '''Fit polynomials to lane lines using convolutional window positioning method'''

    left_lane_pts = []
    right_lane_pts = []

    ######### Compute inital values for level 0
    # First find the two starting positions for the left and right lane windows by using np.sum
    # to get the vertical image slice and then np.convolve the vertical image slice with the
    # window template.
    # Sum quarter bottom of image to get slice, could use a different ratio
    l_sum = np.sum(image[int(3*image.shape[0]/4):,:int(image.shape[1]/2)], axis=0)
    l_center = np.argmax(np.convolve(np.ones(window_width),l_sum))-window_width/2
    r_sum = np.sum(image[int(3*image.shape[0]/4):,int(image.shape[1]/2):], axis=0)
    r_center = np.argmax(np.convolve(np.ones(window_width),r_sum))-window_width/2+int(warped.shape[1]/2)

    # Identify the nonzero pixels in x and y within the window
    good_left_pts = get_windowed_pts(image, l_center, window_width, window_height, out_img, level=0)
    good_right_pts = get_windowed_pts(image, r_center, window_width, window_height, out_img, level=0)

    # Append first layer pixel inds
    left_lane_pts.append(good_left_pts)
    right_lane_pts.append(good_right_pts)

    
    ######### Go through each subsequent level using convolutional window positioning method
    for level in range(1,(int)(image.shape[0]/window_height)):
        
        # convolve vertically summed slice with rectangular window
        conv_signal = compute_conv_signal(image, window_width, window_height, level)
        
        # find window center locations yielding the most pixels within sliding margin
        l_center = compute_window_center(image, l_center, window_width, margin, conv_signal)
        r_center = compute_window_center(image, r_center, window_width, margin, conv_signal)
        
        # Identify the nonzero pixels within the windows
        good_left_pts = get_windowed_pts(image, l_center, window_width, window_height, out_img, level=level)
        good_right_pts = get_windowed_pts(image, r_center, window_width, window_height, out_img, level=level)

        # Append pixel pts
        left_lane_pts.append(good_left_pts)
        right_lane_pts.append(good_right_pts)
    
    
    # Concatenate the arrays of indices
    left_lane_pts = np.concatenate(left_lane_pts)
    right_lane_pts = np.concatenate(right_lane_pts)

    # Draw polynomial lane curves on visualization image
    draw_polyfit(left_lane_pts, out_img)
    draw_polyfit(right_lane_pts, out_img)
        
    return left_lane_pts, right_lane_pts


def get_lane_pts_pixelmean(image, nwindows, margin, minpix, out_img=None):
    '''Fit polynomials to lane lines using pixel mean window positioning method'''

    left_lane_pts = []
    right_lane_pts = []
    window_height = image.shape[0]//nwindows
    
    ######### Compute inital values for left and right window positions
    # Take a histogram of the bottom half of the image
    histogram = np.sum(image[image.shape[0]//2:,:], axis=0)
    # Find the peak of the left and right halves of the histogram
    # These will be the starting point for the left and right lines
    midpoint = histogram.shape[0]//2
    leftx_current = np.argmax(histogram[:midpoint])
    rightx_current = np.argmax(histogram[midpoint:]) + midpoint

    ######### Go through each level using pixel mean window positioning method
    for level in range(nwindows):

        # Identify the nonzero pixels in x and y within the window
        good_left_pts = get_windowed_pts(image, leftx_current, margin*2, window_height, out_img, level=level)
        good_right_pts = get_windowed_pts(image, rightx_current, margin*2, window_height, out_img, level=level)
        # Append these indices to the lists
        left_lane_pts.append(good_left_pts)
        right_lane_pts.append(good_right_pts)
        # If you found > minpix pixels, recenter next window on their mean position
        if len(good_left_pts) > minpix:
            leftx_current = np.int(good_left_pts[:,1].mean())
        if len(good_right_pts) > minpix:        
            rightx_current = np.int(good_right_pts[:,1].mean())


    # Concatenate the arrays of indices
    left_lane_pts = np.concatenate(left_lane_pts)
    right_lane_pts = np.concatenate(right_lane_pts)

    # Draw polynomial lane curves on visualization image
    draw_polyfit(left_lane_pts, out_img)
    draw_polyfit(right_lane_pts, out_img)

    return left_lane_pts, right_lane_pts


def get_polymargin_pts(image, fit_coefs, margin, out_img=None):
    
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = np.transpose(image.nonzero())
    nonzeroy = nonzero[:,0]
    nonzerox = nonzero[:,1]
    
    # Identify the nonzero pixels in x and y within the window
    windowed_pts = nonzero[(nonzerox > (fit_coefs[0]*(nonzeroy**2)
                                        + fit_coefs[1]*nonzeroy
                                        + fit_coefs[2]
                                        - margin)) &
                           (nonzerox < (fit_coefs[0]*(nonzeroy**2)
                                        + fit_coefs[1]*nonzeroy
                                        + fit_coefs[2]
                                        + margin)), :]
    
    if out_img is not None:
        
        # Generate x and y values for plotting the search boundaries
        ploty = np.arange(0,image.shape[0],10)
        fitx = fit_coefs[0]*ploty**2 + fit_coefs[1]*ploty + fit_coefs[2]

        # Draw the search boundaries
        pts = np.stack((np.int32(np.round(fitx - margin)), np.int32(ploty)), axis=1).reshape((-1,1,2))
        cv2.polylines(out_img,[pts],False,(0,255,0), 3)
        pts = np.stack((np.int32(fitx + margin), np.int32(ploty)), axis=1).reshape((-1,1,2))
        cv2.polylines(out_img,[pts],False,(0,255,0), 3)
    
    return windowed_pts
    

def get_lane_pts_poly(image, prev_left_fit, prev_right_fit, margin, out_img=None):
    '''Fit polynomials to lane lines using previous polynomial with margin pixel selection method'''
    
    left_lane_pts = get_polymargin_pts(image, fit_coefs=prev_left_fit, margin=margin, out_img=out_img)
    right_lane_pts = get_polymargin_pts(image, fit_coefs=prev_right_fit, margin=margin, out_img=out_img)
    
    # Draw polynomial lane curves on visualization image
    draw_polyfit(left_lane_pts, out_img)
    draw_polyfit(right_lane_pts, out_img)

    return left_lane_pts, right_lane_pts
In [10]:
plt.close('all')

examplefiles = glob.glob('test_images/*.jpg')

for examplefile in examplefiles:#['test_images/test4.jpg']:#examplefiles:
    
    # Load a test image
    img = mpimg.imread(examplefile)

    # Correct for camera distortion
    dst = cv2.undistort(img, mtx, dist, None, mtx)

    # Get binary mask image
    combined,_ = get_pixel_mask(dst)

    # Warp binary mask images
    warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)

    # Select lane line points using convolution window positioning
    window_width = 100 
    window_height = 80 # Break image into 9 vertical layers since image height is 720
    margin = 50 # How much to slide left and right for searching
    warped_conv = np.dstack([warped]*3) # color image to draw on for visualization of polyfit
    left_pts_conv, right_pts_conv = get_lane_pts_conv(warped, window_width, window_height, margin,
                                                      out_img=warped_conv)
    
    # Select lane line points using pixel mean window positioning
    nwindows = 9 # Choose the number of sliding windows
    margin = 100 # Set the width of the windows +/- margin
    minpix = 350 # Set minimum number of pixels found to recenter window
    warped_pmean = np.dstack([warped]*3) # color image to draw on for visualization of polyfit
    left_pts_pmean, right_pts_pmean = get_lane_pts_pixelmean(warped, nwindows, margin, minpix, out_img=warped_pmean)
    
    # Select lane line points using existing polynomial with margin
    margin = 100 # Set search width surrounding previous lane line curve
    left_fit = np.polyfit(left_pts_pmean[:, 0], left_pts_pmean[:, 1], 2)
    right_fit = np.polyfit(right_pts_pmean[:, 0], right_pts_pmean[:, 1], 2)
    warped_poly = np.dstack([warped]*3) # color image to draw on for visualization of polyfit
    left_pts_poly, right_pts_poly = get_lane_pts_poly(warped, left_fit, right_fit, margin, out_img=warped_poly)

    # Plot all lane pixel fitting methods
    fig, ax = plt.subplots(2, 2, figsize=(16, 10))
    ax[0,0].imshow(warped_conv)
    ax[0,0].set_title(examplefile + ' lane fit conv\n(not used in pipeline)', fontsize=15)
    ax[0,1].imshow(warped_pmean)
    ax[0,1].set_title(examplefile + ' lane fit pixelmean\n(used in pipeline on reset)', fontsize=15)
    ax[1,1].imshow(warped_poly)
    ax[1,1].set_title(examplefile + ' lane fit poly\n(used in pipeline nominally)', fontsize=15)
    fig.delaxes(ax[1,0])
    
    name, ext = os.path.splitext(os.path.basename(examplefile))
    fig.savefig('examples/' + name + '_polyfit' + ext, bbox_inches='tight')

6. Determine the curvature of the lane and vehicle position with respect to center.

In [11]:
def fit_pts(lane_pts, ym_per_pix=1, xm_per_pix=1):
    '''
    Compute 2nd order polynomial curve fit of points with optional
    conversion of pixel units to meters.
    Default conversion is no conversion (1m/pixel)
       
    Parameters
    ----------
    lane_pts : nx2 numpy array
        array of (y,x) positions of this lane line's pixels
    ym_per_pix : float
        meters per pixel in the y dimension
    xm_per_pix : float
        meters per pixel in the x dimension
        
    Returns
    -------
    fit_m : float
        2nd order polynomial fit coefficients in meters
    '''
    
    # Convert pixels to meters
    lane_pts_m = lane_pts*np.array([[ym_per_pix, xm_per_pix]])
    
    # Fit new polynomials to x,y in world space
    fit_m = np.polyfit(lane_pts_m[:,0], lane_pts_m[:,1], 2)
    
    return fit_m


def compute_curvature(fit, yeval):
    '''
    Compute radius of curvature of a 2nd order polynomial at yeval.
       
    Parameters
    ----------
    yeval : float
        y value at which to compute radius of curvature.
        
    Returns
    -------
    curverad : float
        radius of curvature at yeval
    '''
    
    # Calculate radius of curvature at yeval
    curverad = ((1 + (2*fit[0]*yeval + fit[1])**2)**1.5) / np.abs(2*fit[0])
    
    return curverad
In [12]:
plt.close('all')

examplefiles = glob.glob('test_images/*.jpg')

for examplefile in examplefiles:#['test_images/test4.jpg']:#examplefiles:
    
    # Load a test image
    img = mpimg.imread(examplefile)

    # Correct for camera distortion
    dst = cv2.undistort(img, mtx, dist, None, mtx)

    # Get binary mask image
    combined,_ = get_pixel_mask(dst)

    # Warp binary mask images
    warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
    
    # Select lane line points using pixel mean window positioning
    nwindows = 9 # Choose the number of sliding windows
    margin = 100 # Set the width of the windows +/- margin
    minpix = 350 # Set minimum number of pixels found to recenter window
    warped_pmean = np.dstack([warped]*3) # color image to draw on for visualization of outcome
    left_pts, right_pts = get_lane_pts_pixelmean(warped, nwindows, margin, minpix, out_img=warped_pmean)

    # Compute polynomial fit in meters
    ym_per_pix = 30/720
    xm_per_pix = 3.7/700
    left_fit_m = fit_pts(left_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
    right_fit_m = fit_pts(right_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
    
    # Compute radii of curvature for each lane line
    yeval_m = (warped.shape[0] - 1) * ym_per_pix # evaluate at bottom of image (car position)
    left_curverad = compute_curvature(left_fit_m, yeval_m)
    right_curverad = compute_curvature(right_fit_m, yeval_m)
    nleft = len(left_pts)
    nright = len(right_pts)
    curverad = (left_curverad if nleft > nright else right_curverad)
    
    # Compute car's offset from center of lane in meters
    left_m, right_m = np.polyval(left_fit_m, yeval_m), np.polyval(right_fit_m, yeval_m)
    lane_pos_m = (left_m + right_m) / 2
    car_pos_m = (warped.shape[1] * xm_per_pix) / 2
    car_offset = car_pos_m - lane_pos_m # negative means car is left of center
    
    # Plot lane line fit and radius of curvature
    plt.figure(figsize=(8,5))
    plt.imshow(warped_pmean)
    titlestr = ('{} lane fit\n left radius = {}m, right radius = {}m\n best radius = {}m, offset = {:.2f}m'
                .format(examplefile, np.int(left_curverad), np.int(right_curverad), np.int(curverad),
                        car_offset))
    plt.title(titlestr, fontsize=15)
    
    name, ext = os.path.splitext(os.path.basename(examplefile))
    plt.gcf().savefig('examples/' + name + '_curverad' + ext, bbox_inches='tight')

7. Warp the detected lane boundaries back onto the original image.

and

8. Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.

In [13]:
def draw_lane_region(left_fit, right_fit, out_img):
    '''Draw lane region onto out_img'''

    # Generate x and y values for plotting the polynomials
    ploty = np.arange(0, out_img.shape[0], 1)
    left_fitx = np.polyval(left_fit, ploty)
    right_fitx = np.polyval(right_fit, ploty)

    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
    pts = np.hstack((pts_left, pts_right))

    # Draw the lane onto the warped blank image
    cv2.fillPoly(out_img, np.int_([pts]), (0,255, 0))
In [14]:
plt.close('all')

examplefiles = glob.glob('test_images/*.jpg')

for examplefile in examplefiles:
    
    # Load a test image
    img = mpimg.imread(examplefile)

    # Correct for camera distortion
    dst = cv2.undistort(img, mtx, dist, None, mtx)

    # Get binary mask image
    combined,_ = get_pixel_mask(dst)

    # Warp binary mask images
    warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
    
    # Select lane line points using pixel mean window positioning
    nwindows = 9 # Choose the number of sliding windows
    margin = 100 # Set the width of the windows +/- margin
    minpix = 350 # Set minimum number of pixels found to recenter window
    warped_pmean = np.dstack([warped]*3) # color image to draw on for visualization of outcome
    left_pts, right_pts = get_lane_pts_pixelmean(warped, nwindows, margin, minpix, out_img=warped_pmean)

    # Compute polynomial fit in meters
    ym_per_pix = 30/720
    xm_per_pix = 3.7/700
    left_fit_m = fit_pts(left_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
    right_fit_m = fit_pts(right_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
    
    # Compute radii of curvature for each lane line
    yeval_m = (warped.shape[0] - 1) * ym_per_pix # evaluate at bottom of image (car position)
    left_curverad = compute_curvature(left_fit_m, yeval_m)
    right_curverad = compute_curvature(right_fit_m, yeval_m)
    nleft = len(left_pts)
    nright = len(right_pts)
    curverad = (left_curverad if nleft > nright else right_curverad)
    
    # Compute car's offset from center of lane in meters
    left_m, right_m = np.polyval(left_fit_m, yeval_m), np.polyval(right_fit_m, yeval_m)
    lane_pos_m = (left_m + right_m) / 2
    car_pos_m = (warped.shape[1] * xm_per_pix) / 2
    car_offset = car_pos_m - lane_pos_m # negative means car is left of center
    
    # Compute polynomial fit in pixels
    left_fit = np.polyfit(left_pts[:, 0], left_pts[:, 1], 2)
    right_fit = np.polyfit(right_pts[:, 0], right_pts[:, 1], 2)
    
    # Draw lane region on unwarped color image
    lane_img = np.dstack([np.zeros_like(warped, np.uint8)]*3) # blank color image for drawing
    draw_lane_region(left_fit, right_fit, lane_img)
    lane_img_unwarped = cv2.warpPerspective(lane_img, Minv, imgsize) # unwarp top-down lane image
    lane_overlay = cv2.addWeighted(dst, 1, lane_img_unwarped, 0.3, 0) # mix with car-perpective image
    
    # Draw radius of curvature and lane offset text onto lane overlay
    imgtext = 'Radius of Curvature = {}m'.format(np.int(curverad))
    cv2.putText(lane_overlay, imgtext, (150,100), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
    side = ('left' if car_offset < 0 else 'right')
    imgtext = 'Vehicle is {:.2f}m {} of center'.format(np.abs(car_offset), side)
    cv2.putText(lane_overlay, imgtext, (150,200), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
    
    # Plot lane line fit and radius of curvature
    fig, ax = plt.subplots(1, 2, figsize=(16, 5))
    ax[0].imshow(lane_img)
    ax[0].set_title(examplefile + ' lane region top-down', fontsize=15)
    ax[1].imshow(lane_overlay)
    ax[1].set_title(examplefile + ' lane region overlay', fontsize=15)
    
    name, ext = os.path.splitext(os.path.basename(examplefile))
    plt.gcf().savefig('examples/' + name + '_regionoverlay' + ext, bbox_inches='tight')

Define pipeline function and Run on video

In [15]:
from collections import deque

# Define a class to receive the characteristics of each line detection
class Line():
    def __init__(self):
        # was the line detected in the last iteration?
        self.detected = False
        #polynomial coefficient exponential moving average
        self.avg_fit = None
        #polynomial coefficients for the most recent fits
        self.current_fit = [np.array([False])]  
        #radius of curvature of the line in some units
        self.radius_of_curvature = None 
        #distance in meters of vehicle center from the line
        self.line_base_pos = None 
        #difference in fit coefficients between last and new fits
        self.diffs = np.array([0,0,0], dtype='float') 
        #(y,x) pairs for all detected line pixels
        self.allpts = None  
        
leftline = Line()
rightline = Line()
In [16]:
def process_image(img):
    
    global leftline
    global rightline
    
    # Correct for camera distortion
    dst = cv2.undistort(img, mtx, dist, None, mtx)
    
    # Get binary mask image
    combined,_ = get_pixel_mask(dst)

    # Warp binary mask image
    warped = cv2.warpPerspective(combined, M, imgsize, flags=cv2.INTER_LINEAR)
    
    if leftline.detected and rightline.detected:
        
        # Select lane line points using existing polynomial with margin
        left_fit = leftline.current_fit
        right_fit = rightline.current_fit
        margin = 100 # Set search width surrounding previous lane line curve
        left_pts, right_pts = get_lane_pts_poly(warped, left_fit, right_fit, margin)
        
    else:
        
        # Select lane line points using pixel mean window positioning
        nwindows = 9 # Choose the number of sliding windows
        margin = 100 # Set the width of the windows +/- margin
        minpix = 350 # Set minimum number of pixels found to recenter window
        left_pts, right_pts = get_lane_pts_pixelmean(warped, nwindows, margin, minpix)
    
    # Compute polynomial fit in meters
    ym_per_pix = 30/720
    xm_per_pix = 3.7/700
    left_fit_m = fit_pts(left_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
    right_fit_m = fit_pts(right_pts, ym_per_pix=ym_per_pix, xm_per_pix=xm_per_pix)
    
    # Compute radii of curvature for each lane line
    yeval_m = (warped.shape[0] - 1) * ym_per_pix # evaluate at bottom of image (car position)
    left_curverad = compute_curvature(left_fit_m, yeval_m)
    right_curverad = compute_curvature(right_fit_m, yeval_m)
    nleft = len(left_pts)
    nright = len(right_pts)
    
    # Compute car's offset from center of lane in meters
    left_m, right_m = np.polyval(left_fit_m, yeval_m), np.polyval(right_fit_m, yeval_m)
    lane_pos_m = (left_m + right_m) / 2
    car_pos_m = (warped.shape[1] * xm_per_pix) / 2
    car_offset = car_pos_m - lane_pos_m # negative means car is left of center
    
    
    # Compute polynomial fit in pixels
    left_fit = np.polyfit(left_pts[:, 0], left_pts[:, 1], 2)
    right_fit = np.polyfit(right_pts[:, 0], right_pts[:, 1], 2)
    
    # Sanity Check
    curve_diff = left_curverad - right_curverad
    curve_diff_ok = np.abs(curve_diff) < 500
    curverad_debug = (left_curverad if nleft > nright else right_curverad)
#     print('curve_diff', curve_diff)
#     print('curverad', curverad_debug)
    curve_ok = curve_diff_ok or (curverad > 3000)
    y = np.linspace(0, yeval_m)
    separations = np.polyval(right_fit_m, y) - np.polyval(left_fit_m, y)
    separation_ok = np.abs(separations.mean() - 3.7) < 0.5
#     print('separations.mean()', separations.mean())
    parallel_ok = separations.std() < 0.06
#     print('separations.std()', separations.std())
    pixel_count_ok = (nleft > 5000) and (nright > 5000)
#     print('nleft', nleft, 'nright', nright)
    
    # For now, only using lane line separation check
    if separation_ok:
        
        # store detected characteristics for use on future sanity failures
        leftline.detected = True
        rightline.detected = True
        leftline.allpts = left_pts
        rightline.allpts = right_pts
        leftline.radius_of_curvature = left_curverad
        rightline.radius_of_curvature = right_curverad
        leftline.line_base_pos = left_m
        rightline.line_base_pos = right_m
        leftline.current_fit = left_fit
        rightline.current_fit = right_fit
        if leftline.avg_fit is None:
            leftline.avg_fit = left_fit
            rightline.avg_fit = right_fit
        else:
            leftline.avg_fit = left_fit*0.1 + leftline.avg_fit*0.9 # exponential moving average filter
            rightline.avg_fit = right_fit*0.1 + rightline.avg_fit*0.9 # exponential moving average filter
            left_fit = leftline.avg_fit
            right_fit = rightline.avg_fit
        
    else:
        
        # sanity failed, load last known good values
        leftline.detected = False
        leftline.detected = False
        
        if leftline.radius_of_curvature is not None:
            left_fit = leftline.avg_fit
            right_fit = rightline.avg_fit
            left_curverad = leftline.radius_of_curvature
            right_curverad = rightline.radius_of_curvature
            left_m = leftline.line_base_pos
            right_m = rightline.line_base_pos
            lane_pos_m = (left_m + right_m) / 2
            car_pos_m = (warped.shape[1] * xm_per_pix) / 2
            car_offset = car_pos_m - lane_pos_m # negative means car is left of center
        
    
    # Draw lane region on unwarped color image
    lane_img = np.dstack([np.zeros_like(warped, np.uint8)]*3) # blank color image for drawing
    draw_lane_region(left_fit, right_fit, lane_img)
    lane_img_unwarped = cv2.warpPerspective(lane_img, Minv, imgsize) # unwarp top-down lane image
    lane_overlay = cv2.addWeighted(dst, 1, lane_img_unwarped, 0.3, 0) # mix with car-perpective image
    
    # Draw radius of curvature and lane offset text onto lane overlay
    best_curverad = (left_curverad if nleft > nright else right_curverad)
    imgtext = 'Radius of Curvature = {}m'.format(np.int(best_curverad))
    cv2.putText(lane_overlay, imgtext, (150,100), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
    side = ('left ' if car_offset < 0 else 'right')
    imgtext = 'Vehicle is {:.2f}m {} of center'.format(np.abs(car_offset), side)
    cv2.putText(lane_overlay, imgtext, (150,200), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255), 7)
    
    # Debug text
#     imgtext = 'curve_diff {:.0f}, curverad {:.0f}, sepmean {:.2f}, sepstd {:.4f}'.format(curve_diff,
#                                                                                          curverad_debug,
#                                                                                          separations.mean(),
#                                                                                          separations.std())
#     cv2.putText(lane_overlay, imgtext, (0,30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,255), 3)
    
    return lane_overlay
In [17]:
from moviepy.editor import VideoFileClip
video_output = 'project_video_out.mp4'
clip1 = VideoFileClip("project_video.mp4")
white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(video_output, audio=False)
[MoviePy] >>>> Building video project_video_out.mp4
[MoviePy] Writing video project_video_out.mp4
100%|█████████▉| 1260/1261 [06:59<00:00,  3.05it/s]
[MoviePy] Done.
[MoviePy] >>>> Video ready: project_video_out.mp4 

CPU times: user 10min, sys: 2.18 s, total: 10min 2s
Wall time: 7min
In [18]:
from IPython.display import HTML
HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(video_output))
Out[18]:
In [ ]: